/* Private includes ----------------------------------------------------------*/
#include "vmc.h"
#include "functions.h"
/* Private define ------------------------------------------------------------*/
/* Private typedef -----------------------------------------------------------*/
/* Private variables ---------------------------------------------------------*/
VMC right_vmc;
VMC left_vmc;

void Vmc_Cal(FOOT *foot, VMC *vmc)
{
  float C1 = arm_cos_f32(foot->phi0 - foot->phi3);
  float C2 = arm_cos_f32(foot->phi0 - foot->phi2);
  float S1 = arm_sin_f32(foot->phi0 - foot->phi3);
  float S2 = arm_sin_f32(foot->phi0 - foot->phi2);
  float S3 = arm_sin_f32(foot->phi1 - foot->phi2);
  float S4 = arm_sin_f32(foot->phi3 - foot->phi4);
  float S5 = arm_sin_f32(foot->phi2 - foot->phi3);

  vmc->T1 = -(L1 * S3 * (vmc->T * C1 + vmc->F * foot->L0 * S1)) / (foot->L0 * S5);
  vmc->T2 = -(L4 * S4 * (vmc->T * C2 + vmc->F * foot->L0 * S2)) / (foot->L0 * S5);
}